#include "GlobalVar.h"
#include "BasicFun.h"
#include "MainProcess.h"
#include "FillSpecialLine.h"
#include "CircleIsland.h"
#include "FindLine.h"
#include "speedC.h"

#define SLACK_TH 5
#define START_ROW 65
#define UP_EDGE_ZERO 0
#define UP_SLACK 20
#define UP_EDGE_FORK 15
#define TEST_POINT_ROW_L 33
#define TEST_POINT_COL_L 0
#define	TEST_POINT_ROW_R 33
#define TEST_POINT_COL_R 187

#define END_ROW 35

#define OFFSET 50

#define MAXNUM_FDQ 4
typedef struct
{
	char front;
	char rear;
	char queue[MAXNUM_FDQ];
}Forks_Direct_queue;
Forks_Direct_queue FD_queue;

Point FPointA = { DOWN_EDGE,LEFT_EDGE };
Point FPointB = { DOWN_EDGE,RIGHT_EDGE };
Point FPointC = { DOWN_EDGE,MID };

//========================内部基础函数，无需引用===========================//

//================================================================//
//  @brief :	初始化队
//  @param :		void
//  @return :	void
//  @note :		void
//================================================================//
void Init_Queue()
{
	unsigned char i = 0;
	FD_queue.front = FD_queue.rear = 0;
	for (i = 0; i < MAXNUM_FDQ; i++)
	{
		FD_queue.queue[i] = FN;
	}
}
//================================================================//
//  @brief :	入队
//  @param :	char FD's direct
//  @return :	void
//  @note :		void
//================================================================//
void In_Queue(char direct)
{
	if ((FD_queue.rear + 1) % MAXNUM_FDQ == FD_queue.front)
		return;
	FD_queue.rear = (FD_queue.rear + 1) % MAXNUM_FDQ;
	FD_queue.queue[FD_queue.rear] = direct;
}
//================================================================//
//  @brief :	出队
//  @param :	void
//  @return :	队元素char direct
//  @note :		void
//================================================================//
char OUT_Queue()
{
	if (FD_queue.front == FD_queue.rear)
		return -1;
	FD_queue.front = (FD_queue.front + 1) % MAXNUM_FDQ;
	return FD_queue.queue[FD_queue.front];
}
//================================================================//
//  @brief :	访问队尾元素
//  @param :	void
//  @return :	队元素char direct
//  @note :		void
//================================================================//
char Visit_Queue()
{
	if (FD_queue.front == FD_queue.rear)
		return -1;
	return FD_queue.queue[FD_queue.rear];
}
//================================================================//
//  @brief :	访问队首元素
//  @param :	void
//  @return :	队元素char direct
//  @note :		void
//================================================================//
char Visit_Front_Queue()
{
	if (FD_queue.front == FD_queue.rear)
		return -1;
	return FD_queue.queue[(FD_queue.front + 1) % MAXNUM_FDQ];
}


//================================================================//
//  @brief :	判断是否为边缘
//  @param :		void
//  @return :	1是 0不是
//  @note :		void
//================================================================//
int IsEdge_Fork(int row, int col)
{
	//限幅
	if (row > DOWN_EDGE) row = DOWN_EDGE;
	if (row < UP_EDGE_ZERO) row = UP_EDGE_ZERO;
	if (col <= LEFT_EDGE) col = LEFT_EDGE + 1;
	if (col >= RIGHT_EDGE) col = RIGHT_EDGE - 1;
	if (FindLineType)
	{
		unsigned char* p = image[0] + IMG_COL * (row - 1) + col - 1;
		unsigned char ack = 0;
		ack += *p++ < LightThreshold;
		ack += *p++ < LightThreshold;
		p += (IMG_COL - 2);
		ack += *p++ < LightThreshold;
		ack += *p++ < LightThreshold;
		if (!ack) return 0;
	}
	if (HIGH_TH == ImageEdge[row][col])
		return 1;
	else return 0;
}
//================================================================//
//  @brief  :		向上搜索一个边界点
//  @param  :		void
//  @return :		边界点所在行
//  @note   :		void
//================================================================//
int SearchOneUpEdge_Fork(int row, int col)
{
	for (; row > UP_EDGE_ZERO; --row)
	{
		if (IsEdge_Fork(row, col))
			return row;
	}
	return UP_EDGE_ZERO;
}
//================================================================//
//  @brief  :		向右搜索一个边界点
//  @param  :		void
//  @return :		边界点所在列
//  @note   :		void
//================================================================//
int SearchOneRightEdge_Fork(int row, int col)
{
	for (; col < RIGHT_EDGE; ++col)
	{
		if (IsEdge_Fork(row, col))
			return col;
	}
	return RIGHT_EDGE;
}
//================================================================//
//  @brief  :		向左搜索一个边界点
//  @param  :		void
//  @return :		边界点所在列
//  @note   :		void
//================================================================//
int SearchOneLeftEdge_Fork(int row, int col)
{
	for (; col > LEFT_EDGE; --col)
	{
		if (IsEdge_Fork(row, col))
			return col;
	}
	return LEFT_EDGE;
}
//================================================================//
//  @brief  :		向左上45搜索一个边界点(锯齿形搜索）
//  @param  :		void
//  @return :		Point类型row&col
//  @note   :		void
//================================================================//
Point SearchLeftUpEdge_Fork(int row, int col)
{
	int i = row, j = col;
	unsigned char k = 0;
	Point Up45;
	while (i > UP_EDGE_ZERO && j > LEFT_EDGE)
	{
		if (IsEdge_Fork(i, j))
		{
			Up45.Row = i;
			Up45.Col = j;
			return Up45;
		}
		if (0 == k)
		{
			k = 1;
			--i;
		}
		else
		{
			k = 0;
			--j;
		}
			
		
	}
	Up45.Row = i;
	Up45.Col = j;
	return Up45;
}
//================================================================//
//  @brief  :		向右上搜索一个边界点(锯齿形x:y=1:2）
//  @param  :		void
//  @return :		边界点所在列
//  @note   :		void
//================================================================//
Point SearchRightUpEdge_Fork(int row, int col)
{
	int i = row, j = col;
	unsigned char k = 0;
	Point Up45;
	while (i > UP_EDGE_ZERO && j < RIGHT_EDGE)
	{
		if (IsEdge_Fork(i, j))
		{
			Up45.Row = i;
			Up45.Col = j;
			return Up45;
		}
		if (0 == k)
		{
			k = 1;
			--i;
		}
		else
		{
			k = 0;
			++j;
		}
	}
	Up45.Row = i;
	Up45.Col = j;
	return Up45;
}
//================================================================//
//  @brief  :		由AB点求C点
//  @param  :		Point PntA, Point PntB（不能反）char model:1:需要向上判断；0：不需要向上，已经是处理过后的点了
//  @return :		char 0=none,1=get
//  @note   :		void
//================================================================//
char GetC_Fork_TwoPnt(Point tmp_C1, Point tmp_C2,char model)
{
	Point tmp_C11 = tmp_C1, tmp_C22 = tmp_C2;
	if (model)
	{
		
		tmp_C1.Col += 5;
		tmp_C2.Col -= 5;
		tmp_C1=SearchRightUpEdge_Fork(tmp_C1.Row,tmp_C1.Col);
		tmp_C2 = SearchLeftUpEdge_Fork(tmp_C2.Row, tmp_C2.Col);
		if (tmp_C1.Row <= SLACK_TH || tmp_C2.Row <= SLACK_TH)
			return 0;
		else if (tmp_C11.Row - tmp_C1.Row < SLACK_TH || tmp_C22.Row - tmp_C2.Row < SLACK_TH)
			return 0;
		else if (abs(tmp_C1.Row - tmp_C2.Row) > 4*SLACK_TH)
			return 0;
		else;
	}

	unsigned char flag1 = 0, flag2 = 0;
	for (int i = tmp_C1.Row; i <= FPointA.Row; i++)
	{
		tmp_C11.Col = SearchOneRightEdge_Fork(i, tmp_C1.Col);
		if (tmp_C11.Col - tmp_C1.Col > 2 * SLACK_TH)
		{
			flag1 = 1;
			break;
		}

		else
		{
			tmp_C11.Row = i;
			tmp_C1 = tmp_C11;
		}
	}
	for (int j = tmp_C2.Row; j < FPointB.Row; j++)
	{
		tmp_C22.Col = SearchOneLeftEdge_Fork(j, tmp_C2.Col);
		if (tmp_C2.Col - tmp_C22.Col > 2 * SLACK_TH)
		{
			flag2 = 1;
			break;
		}
		else
		{
			tmp_C22.Row = j;
			tmp_C2 = tmp_C22;
		}
	}
	if (!flag1 || !flag2)
		return 0;
	if (abs(tmp_C1.Row - tmp_C2.Row) <2*SLACK_TH && abs(tmp_C1.Col - tmp_C2.Col) < 3*SLACK_TH)
	{
		FPointC.Col = (tmp_C1.Col + tmp_C2.Col) >> 1;
		FPointC.Row = (tmp_C1.Row + tmp_C2.Row) >> 1;
		return 1;
	}
	else
		return 0;
}

//================================================================//
//  @brief  :		由A点求C点
//  @param  :		Point PntA（不能反）char model,同上
//  @return :		char 0=none,1=get
//  @note   :		void
//================================================================//
char GetC_Fork_PntA(Point tmp_C1,char model)
{
	Point tmp_C11 = tmp_C1;
	if (model)
	{
		tmp_C1.Col += 2;
		tmp_C1 = SearchRightUpEdge_Fork(tmp_C1.Row, tmp_C1.Col);
		if (tmp_C1.Row <= SLACK_TH)
			return 0;
		else if (tmp_C11.Row - tmp_C1.Row < SLACK_TH)
			return 0;
		else;
	}

	unsigned char flag1 = 0;
	for (int i = tmp_C1.Row; i <= FPointA.Row; i++)
	{
		tmp_C11.Col = SearchOneRightEdge_Fork(i, tmp_C1.Col+1);
		if (tmp_C11.Col - tmp_C1.Col > SLACK_TH)
		{
			flag1 = 1;
			break;
		}

		else
		{
			tmp_C11.Row = i;
			tmp_C1 = tmp_C11;
		}
	}
	if (!flag1)
		return 0;
	if (abs(tmp_C1.Col - MID) < 2 * SLACK_TH)
	{
		FPointC = tmp_C1;
		return 1;
	}
	else
		return 0;
}
//================================================================//
//  @brief  :		由B点求C点
//  @param  :		Point PntA（不能反）
//  @return :		char 0=none,1=get
//  @note   :		void
//================================================================//
char GetC_Fork_PntB(Point tmp_C2,char model)
{
	Point tmp_C22 = tmp_C2;
	if (model)
	{
		tmp_C2.Col -= 2;
		tmp_C2 = SearchLeftUpEdge_Fork(tmp_C2.Row, tmp_C2.Col);
		if (tmp_C2.Row <= SLACK_TH)
			return 0;
		else if (tmp_C22.Row - tmp_C2.Row < SLACK_TH)
			return 0;
		else;
	}
	
	unsigned char  flag2 = 0;
	for (int j = tmp_C2.Row; j < FPointB.Row; j++)
	{
		tmp_C22.Col = SearchOneLeftEdge_Fork(j, tmp_C2.Col - 1);
		if (tmp_C2.Col - tmp_C22.Col > SLACK_TH)
		{
			flag2 = 1;
			break;
		}
		else
		{
			tmp_C22.Row = j;
			tmp_C2 = tmp_C22;
		}
	}
	if (!flag2)
		return 0;
	if (abs(tmp_C2.Col - MID) < 2 * SLACK_TH)
	{
		FPointC = tmp_C2;
		return 1;
	}
	else
		return 0;
}




//========================功能函数===========================//

//三叉状态，初步构想分成四种
//1.看到三叉，没有丢A、B点
//2.进入三叉，开始丢A、B点
//3.三叉正常补图
//4.出三叉,对应s1
//5.出三叉2，对应s2

//================================================================//
//  @brief  :		求A点
//  @param  :		void
//  @return :		char 0=失败，1=成功
//  @note   :		void
//================================================================//
char GetA_Forks()
{
	int max = LL[DOWN_EDGE];
	Point TmpA = { DOWN_EDGE,max};	
	Point TmpO = { DOWN_EDGE,max };
	float tmp_K = 0;
//	Point end = { -1,-1 };
	switch (Forks_State)
	{
	case 0:
	case 1:
	case 2:
	case 3: 
	case 4:
	case 5:
		if (LeftPnt.ErrRow > DOWN_EDGE-SLACK_TH && LeftPnt.ErrCol<LEFT_EDGE+SLACK_TH)
			return 0;
		else
		{
			for (int i = DOWN_EDGE; i > MAX(LeftPnt.ErrRow,END_ROW); i--)
			{
				if (LL[i] > max)
				{
					max = LL[i];
					TmpA.Row = i;
					TmpA.Col = LL[i];
				}
				if (LL[i] < LEFT_EDGE + SLACK_TH)
				{
					if (i > DOWN_EDGE - SLACK_TH)
						return 0;
					break;
				}				
			}
			if (TmpA.Col == LEFT_EDGE||TmpA.Row<=END_ROW+1)
				return 0;
			else if (TmpA.Row < UP_EDGE + 2*SLACK_TH)
				return 0;
			FPointA = TmpA;
			return 1;			 
		}
		break;
	default:return 0; break;
	}
}
//================================================================//
//  @brief  :		求B点
//  @param  :		void
//  @return :		char 0=失败，1=成功
//  @note   :		void
//================================================================//
char GetB_Forks()
{
	int min = RL[DOWN_EDGE];
	Point TmpB = { DOWN_EDGE,min };
	Point TmpO = { DOWN_EDGE,min };
	float tmp_K = 0;
	switch (Forks_State)
	{
	case 0:
	case 1:
	case 2:
	case 3: 
	case 4:
	case 5:
		if (RightPnt.ErrRow > DOWN_EDGE - SLACK_TH && RightPnt.ErrCol > RIGHT_EDGE-SLACK_TH)
			return 0;
		else
		{
			for (int i = DOWN_EDGE,b=MAX(RightPnt.ErrRow, END_ROW) ; i>b; i--)
			{
				if (RL[i] < min)
				{
					min = RL[i];
					TmpB.Row = i;
					TmpB.Col = RL[i];
				}
				if (RL[i] > RIGHT_EDGE - SLACK_TH)
				{
					if (i > DOWN_EDGE - SLACK_TH)
						return 0;
				}
			}
			if (TmpB.Col == RIGHT_EDGE|| TmpB.Row <= END_ROW+1)
				return 0;
			else if (TmpB.Row < UP_EDGE + 2*SLACK_TH)
				return 0;
			FPointB = TmpB;
			return 1;
		}
		break;
	default:return 0; break;
	}
}


//================================================================//
//  @brief  :		找C点
//  @param  :		void
//  @return :		int8 0=无,1=s1
//  @note   :		void
//================================================================//
char GetC_Forks()
{
	
	switch (Forks_State)
	{

	
		case 0:
//		case 3:
		{
			if (getA_Flag && getB_Flag)
			{
				if (abs(FPointA.Row - FPointB.Row) <= 4 * SLACK_TH)
				{
					return(GetC_Fork_TwoPnt(FPointA, FPointB, 1));
				}
			}
				else return 0;
            break;
		}
        case 3:
        {
			if (getA_Flag || getB_Flag)
			{
				if (getA_Flag && getB_Flag)
				{
					if (abs(FPointA.Row - FPointB.Row) <= 3*SLACK_TH)
					{
						return(GetC_Fork_TwoPnt(FPointA, FPointB, 1));
					}
					else return 0;

				}
				else if (getA_Flag)
				{
					return (GetC_Fork_PntA(FPointA, 1));
				}
				else if (getB_Flag)
				{
					return GetC_Fork_PntB(FPointB, 1);
				}
				else return 0;
			}
            break;
        }
		case 1:
//      case 3:
		case 4:
		{
			if (getA_Flag || getB_Flag)
			{
				if (getA_Flag && getB_Flag)
				{
					if (abs(FPointA.Row - FPointB.Row) <= 3*SLACK_TH)
					{
						return(GetC_Fork_TwoPnt(FPointA, FPointB, 1));
					}
					else return 0;

				}
				else if (getA_Flag)
				{
					return (GetC_Fork_PntA(FPointA, 1));
				}
				else if (getB_Flag)
				{
					return GetC_Fork_PntB(FPointB, 1);
				}
				else return 0;
			}

			else
			{
				FindLineType = 0;
				Point tmp_C1 = { START_ROW,MID - 4 * SLACK_TH }, tmp_C11 = tmp_C1;
				Point tmp_C2 = { START_ROW,MID + 4 * SLACK_TH }, tmp_C22 = tmp_C2;
				tmp_C11.Row = SearchOneUpEdge_Fork(tmp_C1.Row, tmp_C1.Col);
				tmp_C22.Row = SearchOneUpEdge_Fork(tmp_C2.Row, tmp_C2.Col);
				if (tmp_C1.Row <= UP_EDGE_FORK || tmp_C2.Row <= UP_EDGE_FORK)
					return 0;

				if (tmp_C1.Row - tmp_C11.Row <= SLACK_TH && tmp_C2.Row - tmp_C22.Row <= SLACK_TH)
					return 0;
				else if (tmp_C1.Row - tmp_C11.Row > SLACK_TH && tmp_C2.Row - tmp_C22.Row > SLACK_TH)
				{
					return GetC_Fork_TwoPnt(tmp_C11, tmp_C22,0);
				}
				else if (tmp_C1.Row - tmp_C11.Row > SLACK_TH)
				{
					
					return GetC_Fork_PntA(tmp_C11, 0);
				}
				else if (tmp_C2.Row - tmp_C22.Row > SLACK_TH)
				{
					return GetC_Fork_PntB(tmp_C22, 0);
				}
				else return 0;
			}
			break;

		}

		case 2:
		case 5:
		{
			FindLineType = 1;
			Point tmp_C1 = { START_ROW,MID - 2 * SLACK_TH }, tmp_C11 = tmp_C1;
			Point tmp_C2 = { START_ROW,MID + 2 * SLACK_TH }, tmp_C22 = tmp_C1;
			tmp_C11.Row = SearchUpEdge(tmp_C1.Row, tmp_C1.Col);
			tmp_C22.Row = SearchUpEdge(tmp_C2.Row, tmp_C2.Col);
			if (tmp_C1.Row <= UP_EDGE || tmp_C2.Row <= UP_EDGE)
				return 0;

			if (tmp_C1.Row - tmp_C11.Row <= SLACK_TH && tmp_C2.Row - tmp_C22.Row <= SLACK_TH)
				return 0;
			else if(tmp_C1.Row - tmp_C11.Row > SLACK_TH && tmp_C2.Row - tmp_C22.Row > SLACK_TH)
			{
				tmp_C1 = tmp_C11; tmp_C2 = tmp_C22;
				unsigned char flag1 = 0, flag2 = 0;
				for (int i = tmp_C1.Row; i <= START_ROW; i++)
				{
					tmp_C11.Col = SearchRightEdge(i, tmp_C1.Col);
					if (tmp_C11.Col - tmp_C1.Col > SLACK_TH)
					{
						flag1 = 1;
						break;
					}

					else
					{
						tmp_C11.Row = i;
						tmp_C1 = tmp_C11;
					}
				}
				for (int j = tmp_C2.Row; j < FPointB.Row; j++)
				{
					tmp_C22.Col = SearchLeftEdge(j, tmp_C2.Col);
					if (tmp_C2.Col - tmp_C22.Col > SLACK_TH)
					{
						flag2 = 1;
						break;
					}
					else
					{
						tmp_C22.Row = j;
						tmp_C2 = tmp_C22;
					}
				}
				if (!flag1 || !flag2)
					return 0;
				if (abs(tmp_C1.Row - tmp_C2.Row) < 5 && abs(tmp_C1.Col - tmp_C2.Col) < 5)
				{
					FPointC.Col = (tmp_C1.Col + tmp_C2.Col) >> 1;
					FPointC.Row = (tmp_C1.Row + tmp_C2.Row) >> 1;
					return 1;
				}
				else
					return 0;

			}
			else if (tmp_C1.Row - tmp_C11.Row > SLACK_TH)
			{
				if (tmp_C1.Row <= UP_EDGE)
					return 0;
				tmp_C1 = tmp_C11;
				unsigned char flag1 = 0;
				for (int i = tmp_C1.Row; i <= START_ROW; i++)
				{
					tmp_C11.Col = SearchRightEdge(i, tmp_C1.Col);
					if (tmp_C11.Col - tmp_C1.Col > SLACK_TH)
					{
						flag1 = 1;
						break;
					}

					else
					{
						tmp_C11.Row = i;
						tmp_C1 = tmp_C11;
					}
				}
				if (!flag1)
					return 0;
				if (abs(tmp_C1.Col - MIDDLE) < 2*SLACK_TH)
				{
					FPointC = tmp_C1;
					return 1;
				}
				else
					return 0;
			}
			else if (tmp_C2.Row - tmp_C22.Row > SLACK_TH)
			{
				tmp_C2 = tmp_C22;
				unsigned char  flag2 = 0;
				for (int j = tmp_C2.Row; j < START_ROW; j++)
				{
					tmp_C22.Col = SearchLeftEdge(j, tmp_C2.Col);
					if (tmp_C2.Col - tmp_C22.Col > SLACK_TH)
					{
						flag2 = 1;
						break;
					}
					else
					{
						tmp_C22.Row = j;
						tmp_C2 = tmp_C22;
					}
				}
				if (!flag2)
					return 0;
				if (abs(tmp_C2.Col - MID) < 2*SLACK_TH )
				{
					FPointC = tmp_C2;
					return 1;
				}
				else
					return 0;
			}
			else return 0;
		}
		default:
			return 0;
	}	
}
//================================================================//
//  @brief  :		三叉状态转换
//  @param  :		void
//  @return :		void
//  @note   :		void
//================================================================//
void ChangeForkState()
{
	static int count_01 = 0;
	static int count_12 = 0;
	static int count_34 = 0;
	static int count_45 = 0;
	static int count_50 = 0;
	switch (Forks_State)
	{
	case 0:
		count_50=0;
		if (getC_Flag)
		{
			count_01++;
			if(count_01==2&&Distance<30)
			{				
				Forks_State = 1;
				order=1;			
			}
			if (count_01 > 2)
			{
				Forks_State = 1;
				order=1;
			}
				
		}
		else
			count_01 = 0;
		break;
	case 1:
		count_01 = 0;
		if (getC_Flag && (!getA_Flag && !getB_Flag)||g_RoadType==2)
		{
			count_12++;
			if (count_12 > 2)
				Forks_State = 2;
		}
		else
			count_12 = 0;
		break;
	case 2:
		count_12 = 0;
		if (!getC_Flag&&g_RoadType!=2&&Forks_Direct)
			Forks_State = 3;
		break;
	case 3:
		if (getC_Flag)
		{			
				Forks_State = 4;
		}
		break;
	case 4:
//		count_34 = 0;
		if (getC_Flag && (!getA_Flag && !getB_Flag)||g_RoadType==2)
		{
			count_45++;
			if (count_45 > 2)
				Forks_State = 5;
		}
		else
			count_45 = 0;
		break;
	case 5:
		count_34=0;
		if(!getC_Flag)
		{
			count_50++;
			if(count_50>2)
				{
					Forks_State=0;
					FD_Clear_Flag=0;
				}
		}
		else
			count_50=0;
		// if(Dist_ClearFork5Flag)
		// 	{
		// 		Forks_State=0;
		// 		FD_Clear_Flag=0;
		// 	}

	default: 
		break;
	}
}
// //================================================================//
// //  @brief  :		三叉等待状态
// //  @param  :		void
// //  @return :		void
// //  @note   :		void
// //================================================================//
void OnHold()
{
	if(!FD_Now)
	{
		FD_WaitFlag=1;
		if(Distance<50)
		{
			Stop_Handle();	//执行刹车
		}
	}
	else
	{
		FD_WaitFlag=0;
		if(!Get_StopLine)
			Go_Handle();
		
		if(Distance<30)
		{
			Too_Close_Flag=1;
		}
	}

}


//================================================================//
//  @brief  :		获得三叉方向
//  @param  :		void
//  @return :		void
//  @note   :		void
//================================================================//
void GetForkDirect()
{
	switch(Forks_State)
	{
		case 1:
		case 2:
			if(!Forks_Direct)
			{
				if(rount==1)
				{
					if(!Pre_Insert)
					{
						OnHold();
						if(FD_Ready_Flag)
						{
							Forks_Direct=FD_Now;
							FD_Ready_Flag=0;
							order=0;
							In_Queue(Forks_Direct);
						}
					}					
					//预置入：
					else
						Forks_Direct=FD_Now=CL;					
				}
				else if(rount==2)
				{
					if(!Pre_Insert)
					{
						FD_Now=OUT_Queue();
						FD_Now=(FD_Now==FL)?FR:FL;
						Forks_Direct=FD_Now;
					}
					else 
						Forks_Direct=FD_Now=CR;
				}
			}
			break;
//		case 2:								break;
		case 3:	Forks_Direct=FN;			break;
		case 4:	Forks_Direct=FD_Now;		break;
		case 5: 
		case 0:
			if(FD_Clear_Flag)
			{
				if(Forks_State==5)
					FD_Now=FN;
				Forks_Direct=FN;
			}
			break;
	}
}

//================================================================//
//  @brief  :		FN补图
//  @param  :		void
//  @return :		void
//  @note   :		void
//================================================================//
void FN_Handle()
{
	if (Forks_State)
	{
		if (getA_Flag && getB_Flag)
		{
			if (FPointB.Row < DOWN_EDGE - SLACK_TH && FPointA.Row < DOWN_EDGE - SLACK_TH)
			{
				RightPnt = FillLineUp(RL, FPointB.Row + 6, FPointB.Row + 2);
				LeftPnt = FillLineUp(LL, FPointA.Row + 6, FPointA.Row + 2);
				FillMiddleLine();
			}
			else 
				for (int j = DOWN_EDGE; j > UP_EDGE; j--)
				{
					ML[j] = FPointC.Col;
				}	
		}
		else if (getC_Flag)
		{
			for (int j = DOWN_EDGE; j > UP_EDGE; j--)
			{
				ML[j] = FPointC.Col;
			}
		}

	}

}
//================================================================//
//  @brief  :		判断三岔
//  @param  :		void
//  @return :		void
//  @note   :		void
//================================================================//
void Img_ForksJudge()
{
	getA_Flag = GetA_Forks();
	getB_Flag = GetB_Forks();
	getC_Flag = GetC_Forks();
	ChangeForkState();
}





//================================================================//
//  @brief  :		处理三叉
//  @param  :		void
//  @return :		void
//  @note   :		void
//================================================================//
void ForksHandle()
{
	getA_Flag = GetA_Forks();
	getB_Flag = GetB_Forks();
	getC_Flag = GetC_Forks();


	ChangeForkState();
	GetForkDirect();
	


	switch (Forks_State)
	{
		case 1:
		{
			if (Forks_Direct == FL)
			{
				if (LeftPnt.ErrRow < FPointA.Row)
				{
					for (int i = DOWN_EDGE; i > LeftPnt.ErrRow; i--)
					{
						ML[i] = LL[i] + MidOffset[i];
					}
				}
				else
				{
					for (int i = LeftPnt.ErrRow; i > UP_EDGE; i--)
					{
						LL[i] = GetLL(i, LL[i - 1] + 3);
						if (LL[i] < LEFT_EDGE + SLACK_TH)
							break;
						else LeftPnt.ErrRow = i;
					}
					for (int j = DOWN_EDGE; j > LeftPnt.ErrRow; j--)
					{
						ML[j] = LL[j] + MidOffset[j];
						if (ML[j] < LEFT_EDGE + SLACK_TH)
						{
							ML_Count = j;
							break;
						}
					}
				}
				if (ML[LeftPnt.ErrRow] > LEFT_EDGE + SLACK_TH && LeftPnt.ErrRow > UP_EDGE + SLACK_TH)
				{
					if (LeftPnt.ErrRow < DOWN_EDGE - SLACK_TH && (LeastSquare(ML, LeftPnt.ErrRow + 4, LeftPnt.ErrRow + 1) > 0.5))
					{
						LeftPnt = FillLineUp(ML, LeftPnt.ErrRow + 4, LeftPnt.ErrRow + 1);
						ML_Count = LeftPnt.ErrRow;
					}
						
					else
					{
						Connect_Point_Curve(ML, DOWN_EDGE, MIDDLE, TEST_POINT_ROW_L, TEST_POINT_COL_L);
						ML_Count = TEST_POINT_ROW_L;
					}
						
				}
				ML_Count = LeftPnt.ErrRow;	
			}
			else if (Forks_Direct == FR)
			{
				if (RightPnt.ErrRow < FPointB.Row)
				{
					for (int j = DOWN_EDGE; j > RightPnt.ErrRow; j--)
					{
						ML[j] = RL[j] - MidOffset[j];
					}
				
				}
				else
				{
					for (int j = RightPnt.ErrRow; j > UP_EDGE; j--)
					{
						RL[j] = GetRL(j, RL[j - 1] + 3);
						if (RL[j] > RIGHT_EDGE - SLACK_TH)
							break;
						else RightPnt.ErrRow = j;
					}
					for (int j = DOWN_EDGE; j > RightPnt.ErrRow; j--)
					{
						ML[j] = RL[j] - MidOffset[j];
						if (ML[j] > RIGHT_EDGE - SLACK_TH)
						{
							ML_Count = j;
							break;
						}
					}
				}
				if (ML[RightPnt.ErrRow]<RIGHT_EDGE + SLACK_TH && RightPnt.ErrRow>UP_EDGE + SLACK_TH)
				{
					if (RightPnt.ErrRow < DOWN_EDGE - SLACK_TH && LeastSquare(ML, RightPnt.ErrRow + 4, RightPnt.ErrRow + 1) < 0.5)
					{
						RightPnt = FillLineUp(ML, RightPnt.ErrRow + 4, RightPnt.ErrRow + 1);
						ML_Count = RightPnt.ErrRow;
					}
					else
					{
						Connect_Point_Curve(ML, DOWN_EDGE, MIDDLE, TEST_POINT_ROW_R, TEST_POINT_COL_R);			
						ML_Count = TEST_POINT_ROW_R;
					}
								
				}
			}
			else
			{
				FN_Handle();
			}
			break;
		}
		case 2:
		{
			if (Forks_Direct == FL)
			{
				LL[FPointC.Row] = FPointC.Col;
				int i = 0;
				for (i = FPointC.Row; i > UP_EDGE; i--)
				{
					LL[i] = SearchRightEage(i, LL[i] - SLACK_TH);
					if (LL[i] < LEFT_EDGE + SLACK_TH)
						break;
				}
				LeftPnt.ErrRow = i;
				for (i = FPointC.Row; i > LeftPnt.ErrRow; i--)
				{
					ML[i] = LL[i] - MidOffset[i];
					if (ML[i] < LEFT_EDGE + SLACK_TH)
						break;
				}
				if (ML[LeftPnt.ErrRow] > LEFT_EDGE + SLACK_TH && LeftPnt.ErrRow > UP_EDGE + SLACK_TH && LeftPnt.ErrRow< FPointC.Row-SLACK_TH)
				{
					LeftPnt = FillLineUp(ML, LeftPnt.ErrRow + 4, LeftPnt.ErrRow + 1);
				}
				else
				{
					Connect_Point_Curve(ML, DOWN_EDGE, MIDDLE, TEST_POINT_ROW_L, TEST_POINT_COL_L);
					ML_Count = TEST_POINT_ROW_R;
				}
				ML_Count = LeftPnt.ErrRow;
				if (ML[FPointC.Row] < MIDDLE - SLACK_TH)
				{
					FillLineDown(ML, FPointC.Row, FPointC.Row - 4);
				}
			}
			else if (Forks_Direct == FR)
			{
				RL[FPointC.Row] = FPointC.Col;
				int i = 0;
				for (i = FPointC.Row; i > UP_EDGE; i--)
				{
					RL[i] = SearchLeftEage(i, RL[i] + SLACK_TH);
					if (RL[i] > RIGHT_EDGE - SLACK_TH)
						break;
				}
				RightPnt.ErrRow = i;
				for (i = FPointC.Row; i > RightPnt.ErrRow; i--)
				{
					ML[i] = RL[i] + MidOffset[i];
					if (ML[i] > RIGHT_EDGE - SLACK_TH)
						break;
				}
				if (ML[RightPnt.ErrRow]< RIGHT_EDGE - SLACK_TH && RightPnt.ErrRow>UP_EDGE + SLACK_TH && RightPnt.ErrRow < FPointC.Row - SLACK_TH)
				{
					RightPnt= FillLineUp(ML, RightPnt.ErrRow + 4, RightPnt.ErrRow + 1);
				}
				else
				{
					Connect_Point_Curve(ML, DOWN_EDGE, MIDDLE, TEST_POINT_ROW_R, TEST_POINT_COL_R);
					ML_Count = TEST_POINT_ROW_R;
				}
				ML_Count = RightPnt.ErrRow;
				if (ML[FPointC.Row] > MIDDLE - SLACK_TH)
				{
					FillLineDown(ML, FPointC.Row, FPointC.Row - 4);
				}
			}
			else
			{
				FN_Handle();
			}
			break;
		}
		case 3:
		{
			FindLineNormal(0);
			break;
		}
		case 4:
		{
			if (Forks_Direct == FL)
			{
				if (LeftPnt.ErrRow < FPointA.Row)
				{
					for (int i = DOWN_EDGE; i > LeftPnt.ErrRow; i--)
					{
						ML[i] = LL[i] + MidOffset[i];
					}
				}
				else
				{
					for (int i = LeftPnt.ErrRow; i > UP_EDGE; i--)
					{
						LL[i] = GetLL(i, LL[i - 1] + 3);
						if (LL[i] < LEFT_EDGE + SLACK_TH)
							break;
						else LeftPnt.ErrRow = i;
					}
					for (int j = DOWN_EDGE; j > LeftPnt.ErrRow; j--)
					{
						ML[j] = LL[j] + MidOffset[j];
						if (ML[j] < LEFT_EDGE + SLACK_TH)
						{
							ML_Count = j;
							break;
						}
					}
				}
				if (ML[LeftPnt.ErrRow] > LEFT_EDGE + SLACK_TH && LeftPnt.ErrRow > UP_EDGE + SLACK_TH)
				{
					if (LeftPnt.ErrRow < DOWN_EDGE - SLACK_TH && (LeastSquare(ML, LeftPnt.ErrRow + 4, LeftPnt.ErrRow + 1) > 0.5))
					{
						LeftPnt = FillLineUp(ML, LeftPnt.ErrRow + 4, LeftPnt.ErrRow + 1);
						ML_Count = LeftPnt.ErrRow;
					}
						
					else
					{
						Connect_Point_Curve(ML, DOWN_EDGE, MIDDLE, TEST_POINT_ROW_L, TEST_POINT_COL_L);
						ML_Count = TEST_POINT_ROW_L;
					}
						
				}
				ML_Count = LeftPnt.ErrRow;	
			}
			else if (Forks_Direct == FR)
			{
				if (RightPnt.ErrRow < FPointB.Row)
				{
					for (int j = DOWN_EDGE; j > RightPnt.ErrRow; j--)
					{
						ML[j] = RL[j] - MidOffset[j];
					}
				
				}
				else
				{
					for (int j = RightPnt.ErrRow; j > UP_EDGE; j--)
					{
						RL[j] = GetRL(j, RL[j - 1] + 3);
						if (RL[j] > RIGHT_EDGE - SLACK_TH)
							break;
						else RightPnt.ErrRow = j;
					}
					for (int j = DOWN_EDGE; j > RightPnt.ErrRow; j--)
					{
						ML[j] = RL[j] - MidOffset[j];
						if (ML[j] > RIGHT_EDGE - SLACK_TH)
						{
							ML_Count = j;
							break;
						}
					}
				}
				if (ML[RightPnt.ErrRow]<RIGHT_EDGE + SLACK_TH && RightPnt.ErrRow>UP_EDGE + SLACK_TH)
				{
					if (RightPnt.ErrRow < DOWN_EDGE - SLACK_TH && LeastSquare(ML, RightPnt.ErrRow + 4, RightPnt.ErrRow + 1) < 0.5)
					{
						RightPnt = FillLineUp(ML, RightPnt.ErrRow + 4, RightPnt.ErrRow + 1);
						ML_Count = RightPnt.ErrRow;
					}
					else
					{
						Connect_Point_Curve(ML, DOWN_EDGE, MIDDLE, TEST_POINT_ROW_R, TEST_POINT_COL_R);			
						ML_Count = TEST_POINT_ROW_R;
					}
								
				}
			}
			else
			{
				FN_Handle();
			}
			break;
		}
		case 5:
		{
			if (Forks_Direct == FL)
			{
				LL[FPointC.Row] = FPointC.Col;
				int i = 0;
				for (i = FPointC.Row; i > UP_EDGE; i--)
				{
					LL[i] = SearchRightEage(i, LL[i] - SLACK_TH);
					if (LL[i] < LEFT_EDGE + SLACK_TH)
						break;
				}
				LeftPnt.ErrRow = i;
				for (i = FPointC.Row; i > LeftPnt.ErrRow; i--)
				{
					ML[i] = LL[i] - MidOffset[i];
					if (ML[i] < LEFT_EDGE + SLACK_TH)
						break;
				}
				if (ML[LeftPnt.ErrRow] > LEFT_EDGE + SLACK_TH && LeftPnt.ErrRow > UP_EDGE + SLACK_TH)
				{
					LeftPnt = FillLineUp(ML, LeftPnt.ErrRow + 4, LeftPnt.ErrRow + 1);
				}
				ML_Count = LeftPnt.ErrRow;
				if (ML[FPointC.Row] < MIDDLE - SLACK_TH)
				{
					FillLineDown(ML, FPointC.Row, FPointC.Row - 4);
				}
			}
			else if (Forks_Direct == FR)
			{
				RL[FPointC.Row] = FPointC.Col;
				int i = 0;
				for (i = FPointC.Row; i > UP_EDGE; i--)
				{
					RL[i] = SearchLeftEage(i, RL[i] + SLACK_TH);
					if (RL[i] > RIGHT_EDGE - SLACK_TH)
						break;
				}
				RightPnt.ErrRow = i;
				for (i = FPointC.Row; i > RightPnt.ErrRow; i--)
				{
					ML[i] = RL[i] + MidOffset[i];
					if (ML[i] > RIGHT_EDGE - SLACK_TH)
						break;
				}
				if (ML[RightPnt.ErrRow]< RIGHT_EDGE - SLACK_TH && RightPnt.ErrRow>UP_EDGE + SLACK_TH)
				{
					RightPnt= FillLineUp(ML, RightPnt.ErrRow + 4, RightPnt.ErrRow + 1);
				}
				ML_Count = RightPnt.ErrRow;
				if (ML[FPointC.Row] > MIDDLE - SLACK_TH)
				{
					FillLineDown(ML, FPointC.Row, FPointC.Row - 4);
				}
			}
			else
			{
				FN_Handle();
			}
			break;
		}
	}
}